/**
 * File:     wiidevice.m
 * Authors:  Dominic Stalder, Thomas Bruederli
 *
 * Implementation of the objective-c class that connects with the wiiremote framework.
 *
 * This file is part of the Qt-based cross-platform Wii device driver.
 *
 * Copyright (C) 2009-2010, Dominic Stalder & Thomas Bruederli
 * Licensed under the GNU Lesser General Public License (LGPL) as published
 * by the Free Software Foundation at http://www.gnu.org/licenses/lgpl-3.0.txt
 *
 * $Id: wiidevice.m 5 2010-03-03 09:10:12Z roundcube $
 */

#include "wiidevice.h"

#define RAD2DEG(r)	((r)*180.0/M_PI)
#define WIIMOTE_SMOOTH_ALPHA	0.07f


/**
 * Inline C function to do the smooting of orientation values
 * Copied from the wiiuse library (dynamics.c)
 */
inline void wii_apply_smooting(double &val, double &st_val)
{
	// it's possible last iteration was nan or inf, so set it to 0 if that happened */
	if (isnan(st_val) || isinf(st_val))
		st_val = 0.0f;
	
	
	// If the sign changes (which will happen if going from -180 to 180)
	// or from (-1 to 1) then don't smooth, just use the new angle.
	if ((st_val < 0 && val > 0) || (st_val > 0 && val < 0))
	{
		st_val = val;
	}
	else
	{
		val = st_val + (WIIMOTE_SMOOTH_ALPHA * (val - st_val));
		st_val = val;
	}
}


/**
 * Implementation of the wii device on mac os x.
 */
@implementation WiiDevice

/**
 * Class constructor.
 *
 * @param  deviceDriverParam  instance of an <code>WiiDeviceDriver</code>
 */
- (WiiDevice*) initWithCallback: (WiiDeviceDriver*) deviceDriverParam
{
	st_roll = st_pitch = 0.0f;
	
	// initializes the super class
	self = [super init];
	
	// sets the wii device driver based on the param
	deviceDriver = deviceDriverParam;
	
	// sends a log message
	deviceDriver->setLogMessage("Wii Device started.");
	
	// returns this object
	return self;
}

/**
 * Class constructor.
 *
 * @param  deviceDriverParam    instance of an <code>WiiDeviceDriver</code>
 * @param  deviceTypeParam      specifies the type of the wii device
 */
- (WiiDevice*) initWithCallbackAndType: (WiiDeviceDriver*) deviceDriverParam: (int)deviceTypeParam
{
	// initializes the super class
	self = [super init];

	// sets the wii device driver based on the param
	deviceDriver = deviceDriverParam;
	
	// sets the wii device tip based on the param
	deviceType = deviceTypeParam;
	
	// sends a log message
	deviceDriver->setLogMessage("Wii Device started.");
	
	return self;
}

/**
 * Starts a connecting to a wii device and initializes the device discovery.
 */
- (BOOL) connect
{
	// initializes a wii remote discovery object
	wiiDeviceDiscovery = [[WiiRemoteDiscovery alloc] init];
	
	// registrates with the interface
	[ wiiDeviceDiscovery setDelegate:self ];
	
	// starts to discover the wii device
	IOReturn discovery = [ wiiDeviceDiscovery start ];
	
	// check if discovery was started successfully
	if (discovery != kIOReturnSuccess)
	{
		[ self disconnect ];
		return NO;
	}
	else
	{
		// sends the instruction texts to press the red button
		[ self setInstruction:WiiDeviceDriver::MESSAGE_PRESSSYNCBUTTON ];
		
		// adds an observer to register if the expansion port has changed
		[ [ NSNotificationCenter defaultCenter ] addObserver:self selector:@selector(expansionPortChanged:)
														name:@"WiiRemoteExpansionPortChangedNotification" object:nil ];
		
		return YES;
	}
}

/**
 * Stops discovering and closes all the wii device connections.
 */
- (void) disconnect
{
	// closes connection to the wii device
	[ wiiDevice closeConnection ];
	
	// stops discovering
	[ wiiDeviceDiscovery close ];

	// deallocates the wii device discovery instance
	[ wiiDeviceDiscovery dealloc ];
}

/**
 * Deallocates all the instances.
 */
- (void) dealloc
{
	// releases the wii device instance
	[ wiiDevice release ];
	
	// releases the wii device discovery instance
	[ wiiDeviceDiscovery release ];
	
	// deallocates the super object
	[ super dealloc ];
}

/**
 * Wrapper for the <code>setInstruction(const unsigned char instruction)</code> callback method.
 *
 * @param  instruction  instruction text of the wii device
 */
- (void) setInstruction: (unsigned char) instruction
{
	// sends the instruction to the callback object (wii device driver)
	deviceDriver->setInstruction(instruction);
}

/**
 * Wrapper for the <code>setConnectionState(int state)</code> callback method.
 * 
 * @param  state  specifies if the connection state is on (true) or off (false)
 */
-(void) setConnectionState: (BOOL) state
{
	// sends the connection state to the callback object (wii device driver)
	deviceDriver->setConnectionState(state ? WiiDeviceDriver::STATE_CONNECTED : WiiDeviceDriver::STATE_NOTCONNECTED);
}

// implementation of the <code>WiiRemoteDelegate</code> interface
#pragma mark WiiRemoteDiscoveryDelegate implementation

/**
 * Indicates that the connection to the wii device will be started.
 */
- (void) willStartWiimoteConnections
{
	// sends a log message
	deviceDriver->setLogMessage("Wii Device starts connecting...");
}

/**
 * Discovery ended successfully. Checks if the connected wii device is a wii balance board or a 
 * wii controller.
 *
 * @param  wiiDeviceParam  instance of a wii device (balance board or controller) of type <code>WiiRemote</code>
 */
- (void) WiiRemoteDiscovered: (WiiRemote*) wiiDeviceParam
{
	// releases the wii device instance 
	[ wiiDevice release ];
	
	// sends a log message
	deviceDriver->setLogMessage("Wii Device discovered.");

	// checks if wii device is of type wii balance board
	if ((deviceType == WiiDeviceDriver::TYPE_BALANCEBOARD) == [wiiDeviceParam isBalanceBeam])
	{
		wiiDevice = [ wiiDeviceParam retain ];
		
		// registrates with the interface
		[ wiiDeviceParam setDelegate:self ];
		
		// sends the instruction texts to successfull
		[ self setInstruction:WiiDeviceDriver::MESSAGE_SUCCESSFUL ];
		
		// sets the connection state to connected
		[ self setConnectionState:YES ];
	}
	else
	{
		// sends a log message
		deviceDriver->setLogMessage("Wrong Wii Device discovered.");
		
		// restart to discover for a wii device
		[ wiiDeviceDiscovery start ];
	}
}

/**
 * Writes messages and instructions if a discovery error occured.
 */
- (void) WiiRemoteDiscoveryError: (int)code
{
	// sends a log message
	deviceDriver->setLogMessage("Wii Device error occured.");
	
	// sends the instruction texts to press the red button
	[ self setInstruction:WiiDeviceDriver::MESSAGE_PRESSSYNCBUTTON ];
	
	// sets the connection state to disconnected
	[ self setConnectionState:NO ];
	
	// calls the disconnected function
	[ self wiiRemoteDisconnected:nil ];
}

// implementation of the <code>WiiRemoteDelegate</code> interface
#pragma mark WiiRemoteDelegate implementation

/**
 * Triggered when the wii expansion port had a change (eg. a nunchuk was connected).
 *
 * @param  nc  notification message send by the wii device
 */
- (void)expansionPortChanged: (NSNotification *) notification
{
	WiiRemote* wiiDeviceParam = (WiiRemote*)[ notification object ];
	
	// checks if the wii device answering is the one we are connected to
	if (![ [ wiiDevice address ] isEqualToString: [ wiiDeviceParam address ] ])
	{
		return;
	}
	
	// checks if the expasion was of a wii blance board type
	if (([ wiiDeviceParam expansionPortType ] == WiiBalanceBoard
			&& deviceType == WiiDeviceDriver::TYPE_BALANCEBOARD)
				|| [ wiiDeviceParam expansionPortType ] != WiiBalanceBoard)
	{
		if ( [ wiiDevice isExpansionPortAttached ] )
			[ wiiDevice setExpansionPortEnabled:YES ];
		[ wiiDeviceDiscovery stop ];
	}
	else
	{
		// sends a log message
		deviceDriver->setLogMessage("Wrong Wii Device discovered.");
		
		// releases the wii device instance
		[ wiiDevice release ];
		
		// restart to discover for a wii device
		[ wiiDeviceDiscovery start ];
	}
}

/**
 * Triggered when a wii device button has changed (eg. pressed or released).
 *
 * @param  type       specifies the type of the wii device button that was pressed
 * @param  isPressed  specifies if the button is pressed or released
 */
- (void) buttonChanged: (WiiButtonType) buttonType isPressed: (BOOL) isPressed
{
	// translates the button type from enum to int
	int which = 0;	
	
	// checks which button was pressed
	switch (buttonType)
	{
		case WiiRemoteAButton:		which = WiiDeviceDriver::BUTTON_A; break;
		case WiiRemoteBButton:		which = WiiDeviceDriver::BUTTON_B; break;
		case WiiRemoteOneButton:	which = WiiDeviceDriver::BUTTON_1; break;
		case WiiRemoteTwoButton:	which = WiiDeviceDriver::BUTTON_2; break;
		case WiiRemoteMinusButton:	which = WiiDeviceDriver::BUTTON_MINUS; break;
		case WiiRemoteHomeButton:	which = WiiDeviceDriver::BUTTON_HOME; break;
		case WiiRemotePlusButton:	which = WiiDeviceDriver::BUTTON_PLUS; break;
		case WiiRemoteUpButton:		which = WiiDeviceDriver::BUTTON_UP; break;
		case WiiRemoteDownButton:	which = WiiDeviceDriver::BUTTON_DOWN; break;
		case WiiRemoteLeftButton:	which = WiiDeviceDriver::BUTTON_LEFT; break;
		case WiiRemoteRightButton:	which = WiiDeviceDriver::BUTTON_RIGHT; break;
		default:					which = WiiDeviceDriver::BUTTON_NONE; break;
	}
	
	// sends the button pressed event to the callback object
	deviceDriver->setButtonPressed(which, isPressed ? true : false);
}

/**
 * Triggered when the wii device battery level changed. This will always be triggered right
 * after the connection has been established.
 *
 * @param  level  actual battery level of the wii device
 */
- (void) batteryLevelChanged:(double) level
{
	// sets the battery level in the device driver
	deviceDriver->setBatteryLevel((float)level);
}

/**
 * Triggered when the balance board sensor values changed (values in kg).
 *
 * @param  topRight     input value in kg of the top right quadrant
 * @param  bottomRight  input value in kg of the bottom right quadrant
 * @param  topLeft      input value in kg of the top left quadrant
 * @param  bottomLeft   input value in kg of the bottom left quadrant
 */
- (void) pressureChanged:(WiiPressureSensorType) type
			  pressureTR:(float) topRight pressureBR:(float) bottomRight 
			  pressureTL:(float) topLeft pressureBL:(float) bottomLeft
{
	// forward the event data to the callback object
	deviceDriver->setKilograms(topLeft, topRight, bottomRight, bottomLeft);
}


/**
 *
 */
- (void) accelerationChanged:(WiiAccelerationSensorType) type
						accX:(unsigned short) accX
						accY:(unsigned short) accY
						accZ:(unsigned short) accZ
{
	float x0, xg, y0, yg, z0, zg;

	if (type == WiiNunchukAccelerationSensor)
	{
		// TODO: add support for nunchuck
		// deviceDriver->setNunchuckAcceleration(...);
	}
	else
	{
		WiiAccCalibData data = [wiiDevice accCalibData:WiiRemoteAccelerationSensor];
		xg = (float)data.accX_1g;
		yg = (float)data.accY_1g;
		zg = (float)data.accZ_1g;
		x0 = (float)data.accX_zero;
		y0 = (float)data.accY_zero;
		z0 = (float)data.accZ_zero;
	
		double ax = MIN(1.0, MAX(-1.0, ((double)accX - x0) / xg));
		double ay = MIN(1.0, MAX(-1.0, ((double)accY - y0) / yg));
		double az = MIN(1.0, MAX(-1.0, ((double)accZ - z0) / zg));
		
		double roll = RAD2DEG(atan2f(ax, az));
		double pitch = RAD2DEG(atan2f(ay, az));
		
		// smooth values
		wii_apply_smooting(roll, st_roll);
		wii_apply_smooting(pitch, st_pitch);

		// pass calibrated acceleration values to callback object
		deviceDriver->setAcceleration(roll, pitch, 0.0f);
	}
}


- (void) irPointMovedX:(float) px Y:(float) py
{
	// TODO: handle IR events
}


/**
 * Triggered when the wii device has been disconnected.
 *
 * @param device  wii device that has benn disconnected
 */
- (void) wiiRemoteDisconnected: (IOBluetoothDevice*)device
{
	// releases the wii device instance
	[ wiiDevice release ];
	
	// sets wii object to null
	wiiDevice = nil;
	
	// stops to discover a wii device
	[ wiiDeviceDiscovery stop ];
	
	// sends a log message
	deviceDriver->setLogMessage("Lost connection to the Wii Device.");
	
	// sends the instruction text to press the red button
	[ self setInstruction:WiiDeviceDriver::MESSAGE_PRESSSYNCBUTTON ];
	
	// sets the connection state to disconnected
	[ self setConnectionState: NO ];
	
	// restarts to discover the wii device
	[ wiiDeviceDiscovery start ];
}

@end
